#include "LowPass_Fliter.h"

// Tf 
LowPassFliter::LowPassFliter(float time_constant)
{
    Tf = time_constant;
    y_prev = 0.0f;
    timestamp_prev = micros();
}

float LowPassFliter::operator() (float x) 
{
    unsigned long timestamp = micros();
    float dt = (timestamp - timestamp_prev) * 1e-6f;
    
    if(dt < 0.0f) dt = 1e-3f; // micros() will reset each 78 second 
    if (dt > 0.3f) // this time gap is too huge, we don't need to operate it
    {
        y_prev = x;
        timestamp_prev = timestamp;
        return x;
    }

    float alpha = Tf / (Tf + dt); // smooth the line 
    float y = alpha * y_prev + (1.0f - alpha) * x;

    y_prev = y;
    timestamp_prev = timestamp;
    return y;
}
